// 1.包含头文件
#include <iostream>
#include <cmath>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float32.hpp"


using namespace std::chrono_literals;

// 3.自定义节点类；
class ROS2_STM32_Talker : public rclcpp::Node
{
public:
  ROS2_STM32_Talker() : Node("ros2_stm32_talker_node_cpp")
  {
    publisher_ = this->create_publisher<std_msgs::msg::Float32>("voltage_talker", 10);
    timer_ = this->create_wall_timer(500ms, std::bind(&ROS2_STM32_Talker::timer_callback, this));
  }
  // 创建反馈函数
  void timer_callback()
  {
    float voltage;
    // RCLCPP_INFO(this->get_logger(), "---电压修改---");
    std::cout << "Voltage Value :";
    // 避免输入非数字后陷入错误状态
    while (!(std::cin >> voltage))
    {
      std::cin.clear();                                                   // 清除错误状态
      std::cin.ignore(std::numeric_limits<std::streamsize>::max(), '\n'); // 忽略错误输入直到下一个换行符
      std::cout << "" << std::endl;
      RCLCPP_ERROR(this->get_logger(), "非法输入，请输入数字\n");
      std::cout << "Voltage Value :";
    }
    std::cout << "" << std::endl;
    voltage = std::round(voltage * 10);
    auto voltage_data = std_msgs::msg::Float32();
    if (voltage <= 0 && voltage >= -240)
    {
      voltage_data.data = -voltage;
      publisher_->publish(voltage_data);
    }
    else if (voltage > 0 && voltage <= 240)
    {
      voltage_data.data = voltage + 240;
      publisher_->publish(voltage_data);
    }
    else
    {
      RCLCPP_ERROR(this->get_logger(), "数据超出范围，已自动至零\n");
      voltage_data.data = 0;
      publisher_->publish(voltage_data);
    }
  }

private:
  // 创建定时器
  rclcpp::TimerBase::SharedPtr timer_;
  // 创建发布者
  rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr publisher_;
};

int main(int argc, char const *argv[])
{
  // 2.初始化ROS2客户端；
  rclcpp::init(argc, argv);
  auto client = std::make_shared<ROS2_STM32_Talker>();
  // 4.调用spain函数，并传入节点对象指针；
  rclcpp::spin(client);
  // 5.资源释放
  rclcpp::shutdown();
  return 0;
}